//////////////////////////////////////////////////////////////////////////
#pragma once

#include "Math/Vector3f.hpp"
#include "Math/Matrix44f.hpp"
#include "Math/Constants.hpp"


namespace Math
{
	//////////////////////////////////////////////////////////////////////////
	inline Quaternionf::Quaternionf()
	{
	}


	inline Quaternionf::Quaternionf( const Quaternionf& rhs )
	:	mX( rhs.mX )
	,	mY( rhs.mY )
	,	mZ( rhs.mZ )
	,	mW( rhs.mW )
	{
	}


	inline Quaternionf::Quaternionf( float x, float y, float z, float w )
	:	mX( x )
	,	mY( y )
	,	mZ( z )
	,	mW( w )
	{
	}


	//////////////////////////////////////////////////////////////////////////
	inline float Quaternionf::X() const
	{
		return mX;
	}


	inline float Quaternionf::Y() const
	{
		return mY;
	}

	inline float Quaternionf::Z() const
	{
		return mZ;
	}


	inline float Quaternionf::W() const
	{
		return mW;
	}


	inline void Quaternionf::X( float x )
	{
		mX	=	x;
	}


	inline void Quaternionf::Y( float y )
	{
		mY	=	y;
	}


	inline void Quaternionf::Z( float z )
	{
		mZ	=	z;
	}


	inline void Quaternionf::W( float w )
	{
		mW	=	w;
	}


	//////////////////////////////////////////////////////////////////////////
	inline float Quaternionf::operator ()( size_t idx ) const
	{
		assert( idx <= 4 );
		return (&mX)[ idx ];
	}


	inline void Quaternionf::operator ()( size_t idx, float v )
	{
		assert( idx <= 4 );
		(&mX)[ idx ]	=	v;
	}


	//////////////////////////////////////////////////////////////////////////
	inline bool Quaternionf::operator ==( const Quaternionf& rhs ) const
	{
		return mX==rhs.mX && mY==rhs.mY && mZ==rhs.mZ && mW==rhs.mW;
	}


	inline bool Quaternionf::operator !=( const Quaternionf& rhs ) const
	{
		return !((*this)==rhs);
	}


	//////////////////////////////////////////////////////////////////////////
	inline void Quaternionf::Set( float x, float y, float z, float w )
	{
		mX	=	x;
		mY	=	y;
		mZ	=	z;
		mW	=	w;
	}


	inline const Quaternionf& Quaternionf::operator =( const Quaternionf& rhs )
	{
		mX	=	rhs.mX;
		mY	=	rhs.mY;
		mZ	=	rhs.mZ;
		mW	=	rhs.mW;

		return *this;
	}


	//////////////////////////////////////////////////////////////////////////
	inline Quaternionf Quaternionf::operator -() const
	{
		return Quaternionf( -mX, -mY, -mZ, -mW );
	}


	inline Quaternionf Quaternionf::operator +( const Math::Quaternionf& rhs ) const
	{
		return Quaternionf(
			mX+rhs.mX,
			mY+rhs.mY,
			mZ+rhs.mZ,
			mW+rhs.mW	);
	}


	inline Quaternionf Quaternionf::operator *( const Quaternionf& rhs ) const
	{
		Quaternionf	result;

		result.mX	=	(mW * rhs.mX) + (mX * rhs.mW) + (mY * rhs.mZ) - (mZ *rhs. mY);
		result.mY	=	(mW * rhs.mY) + (mY * rhs.mW) + (mZ * rhs.mX) - (mX *rhs. mZ);
		result.mZ	=	(mW * rhs.mZ) + (mZ * rhs.mW) + (mX * rhs.mY) - (mY *rhs. mX);
		result.mW	=	(mW * rhs.mW) - (mX * rhs.mX) - (mY * rhs.mY) - (mZ *rhs. mZ);

		return result;
	}


	inline Quaternionf Quaternionf::operator *( const float rhs ) const
	{
		return Quaternionf(
			mX * rhs,
			mY * rhs,
			mZ * rhs,
			mW * rhs	);
	}


	inline void Quaternionf::operator +=( const Math::Quaternionf& rhs )
	{
		mX	+=	rhs.mX;
		mY	+=	rhs.mY;
		mZ	+=	rhs.mZ;
		mW	+=	rhs.mW;
	}


	inline void Quaternionf::operator *=( const Quaternionf& rhs )
	{
		*this	=	(*this) * rhs;
	}


	//////////////////////////////////////////////////////////////////////////
	inline void Quaternionf::Normalize()
	{
		float	n	=	std::sqrt( Norm( *this ) );
		assert( n!=0 );
		float	s	=	1.0f / n;

		mX	=	mX*s;
		mY	=	mY*s;
		mZ	=	mZ*s;
		mW	=	mW*s;
	}


	inline void Quaternionf::Inverse()
	{
		float	n	=	Norm( *this );
		assert( n!=0 );

		*this	=	Math::Conjugate( *this ) * ( 1.0f/n );
	}


	inline void Quaternionf::Conjugate()
	{
		mX	=	-mX;
		mY	=	-mY;
		mZ	=	-mZ;
	}


	//////////////////////////////////////////////////////////////////////////
	inline Quaternionf Quaternionf::Zero()
	{
		return Quaternionf( 0.0f, 0.0f, 0.0f, 0.0f );
	}


	inline Quaternionf Quaternionf::Identity()
	{
		return Quaternionf( 0.0f, 0.0f, 0.0f, 1.0f );
	}


	inline Quaternionf Quaternionf::Rotation( const Vector3f& axis, float rot )
	{
		Vector3f	normAxis	=	Math::Normalize( axis );
		const float	halfAngle	=	0.5f * rot;
		
		const float	sHalfAngle	=	std::sin( halfAngle );
		const float	cHalfAngle	=	std::cos( halfAngle );

		return Quaternionf(
			normAxis.X() * sHalfAngle,
			normAxis.Y() * sHalfAngle,
			normAxis.Z() * sHalfAngle,
			cHalfAngle		);
	}


	inline Quaternionf Quaternionf::From( const Matrix44f& rhs )
	{
		float	trace	=	rhs( 0 )( 0 ) + rhs( 1 )( 1 ) + rhs( 2 )( 2 );

		if( trace > 0.001f )
		{
			float	s	=	0.5f / std::sqrt( trace + 1.0f );
			return Quaternionf(
				( rhs( 1 )( 2 ) - rhs( 2 )( 1 ) ) * s,
				( rhs( 2 )( 0 ) - rhs( 0 )( 2 ) ) * s,
				( rhs( 0 )( 1 ) - rhs( 1 )( 0 ) ) * s,
				0.25f / s	);
		}
		else
		{
			if(	rhs( 0 )( 0 ) > rhs( 1 )( 1 )	&&
				rhs( 0 )( 0 ) > rhs( 2 )( 2 )	)
			{
				float	s	=	2.0f * std::sqrt( 1.0f + rhs( 0 )( 0 ) - rhs( 1 )( 1 ) - rhs( 2 )( 2 ) );
				return Quaternionf(
					0.25f * s,
					(rhs( 1 )( 0 ) + rhs( 0 )( 1 ) ) / s,
					(rhs( 2 )( 0 ) + rhs( 0 )( 2 ) ) / s,
					(rhs( 1 )( 2 ) - rhs( 2 )( 1 ) ) / s	);
			}
			else if( rhs( 1 )( 1 ) > rhs( 2 )( 2 ) )
			{
				float	s	=	2.0f * std::sqrt( 1.0f + rhs( 1 )( 1 ) - rhs( 0 )( 0 ) - rhs( 2 )( 2 ) );
				return Quaternionf(
					(rhs( 1 )( 0 ) + rhs( 0 )( 1 ) ) / s,
					0.25f * s,
					(rhs( 2 )( 1 ) + rhs( 1 )( 2 ) ) / s,
					(rhs( 2 )( 0 ) - rhs( 0 )( 2 ) ) / s	);
			}
			else
			{
				float	s	=	2.0f * std::sqrt( 1.0f + rhs( 2 )( 2 ) - rhs( 0 )( 0 ) - rhs( 1 )( 1 ) );
				return Quaternionf(
					(rhs( 2 )( 0 ) + rhs( 0 )( 2 ) ) / s,
					(rhs( 2 )( 1 ) + rhs( 1 )( 2 ) ) / s,
					0.25f * s,
					(rhs( 0 )( 1 ) - rhs( 1 )( 0 ) ) / s	);
			}
		}
	}


	//////////////////////////////////////////////////////////////////////////
	inline Vector3f					operator *( const Quaternionf& lhs, const Math::Vector3f& rhs )
	{
		Math::Vector3f	qvec( lhs.X(), lhs.Y(), lhs.Z() );
		Math::Vector3f	uv( Cross( qvec, rhs ) );
		Math::Vector3f	uuv( Cross( qvec, uv ) );
		uv	*=	2.0f * lhs.W();
		uuv	*=	2.0f;

		return rhs + uv + uuv;
	}


	inline float					Dot( const Quaternionf& lhs, const Quaternionf& rhs )
	{
		return lhs.X()*rhs.X() + lhs.Y()*rhs.Y() + lhs.Z()*rhs.Z() + lhs.W()*rhs.W();
	}


	inline Quaternionf				Normalize( const Quaternionf& rhs )
	{
		Quaternionf	result( rhs );
		result.Normalize();
		return result;
	}


	inline Quaternionf				Inverse( const Quaternionf& rhs )
	{
		Quaternionf	result( rhs );
		result.Inverse();
		return result;
	}


	inline Quaternionf				Conjugate( const Quaternionf& rhs )
	{
		Quaternionf	result( rhs );
		result.Conjugate();
		return result;
	}


	inline float					Norm( const Quaternionf& rhs )
	{
		return
			(rhs.X() * rhs.X())	+
			(rhs.Y() * rhs.Y())	+
			(rhs.Z() * rhs.Z())	+
			(rhs.W() * rhs.W());
	}


	inline Quaternionf				Slerp( const Quaternionf& lhs, const Quaternionf& rhs, float p )
	{
		float			omega, cosom, sinom, sclp, sclq;
		Quaternionf		toQ;

		cosom	=	lhs.X()*rhs.X() + lhs.Y()*rhs.Y() + lhs.Z()*rhs.Z() + lhs.W()*rhs.W();

		// deal with floating point error
		if( cosom > 1.0f )
			cosom	=	1.0f;
		else
			if( cosom < -1.0f )
				cosom	=	-1.0f;

		if( cosom < 0.0f )
		{
			toQ		=	-rhs;
			cosom	=	-cosom;
		}
		else
			toQ	=	rhs;

		if( ( 1.0f - cosom ) > 0.0001f )
		{
			omega	=	std::acos( cosom );
			sinom	=	std::sin( omega );
			sclp	=	std::sin( (1.0f - p) * omega ) / sinom;
			sclq	=	std::sin( p * omega ) / sinom;
		}
		else
		{
			// "from" and "to" very close, so do linear interpolation.
			sclp	=	1.0f - p;
			sclq	=	p;
		}

		Quaternionf	result(
			sclp * lhs.X() + sclq * toQ.X(),
			sclp * lhs.Y() + sclq * toQ.Y(),
			sclp * lhs.Z() + sclq * toQ.Z(),
			sclp * lhs.W() + sclq * toQ.W()
			);
		result.Normalize();
		return result;
	}


	inline float					DeltaAngle( const Quaternionf& lhs, const Quaternionf& rhs )
	{
		float	bestAngle	=	ArcDelta( lhs, rhs, Vector3f::XAxis() );
		float	a			=	ArcDelta( lhs, rhs, Vector3f::YAxis() );

		if( fabs(a) > fabs( bestAngle ) )
			bestAngle	=	a;
		a	=	ArcDelta( lhs, rhs, Vector3f::ZAxis() );

		if( fabs( a ) > fabs( bestAngle ) )
			bestAngle = a;

		return bestAngle;
	}


	inline float					ArcDelta( const Quaternionf& lhs, const Quaternionf& rhs, const Vector3f& axis )
	{
		float	dot	=	(lhs*axis) * (rhs*axis);

		if( dot > 1.0f )
			dot	=	1.0f;
		else if( dot < -1.0f )
			dot	=	-1.0f;
		return( std::acos( dot ) );
	}


	inline Quaternionf				RotationTo( const Math::Vector3f& from, const Math::Vector3f& to )
	{
		Vector3f	v0		=	Normalize( from );
		Vector3f	v1		=	Normalize( to );

		const float	d		=	Dot( v0, v1 );
		if( d>=1.0f )
			return Quaternionf::Identity();

		if( d <= -1.0f )
		{
			Vector3f	axis( Vector3f::XAxis() );
			axis	=	Cross( axis, v0 );
			if( MagnitudeSquared( axis )==0.0f )
			{
				axis	=	Vector3f::YAxis();
				axis	=	Cross( axis, v0 );
			}
			return Quaternionf::Rotation( axis, Constants< float >::Pi() );
		}

		const float		s		=	std::sqrt( (1.0f+d)*2.0f );
		const float		invS	=	1.0f / s;
		const Vector3f	c		=	Cross( v0, v1 ) * invS;

		return Quaternionf( c.X(), c.Y(), c.Z(), s * 0.5f );
	}
}
